/*
 *  surveyorsvs.c
 *
 *  David Lobato 2011 <dav.lobato@gmail.com>
 *
 *  This file is part of Surveyor SVS player/stage driver.
 *
 *  Surveyor SVS player/stage driver is free software: you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation, either version 3 of the License, or
 *  (at your option) any later version.
 *
 *   Surveyor SVS player/stage driver is distributed in the hope that it will be useful,
 *   but WITHOUT ANY WARRANTY; without even the implied warranty of
 *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *   GNU General Public License for more details.
 *
 *   You should have received a copy of the GNU General Public License
 *   along with Surveyor SVS player/stage driver.
 *   If not, see <http://www.gnu.org/licenses/>.
 *
 */

#include "surveyorsvs.h"
#include "jpeg_buffer_src.h"
#include "hexdump.h"
#include <stdlib.h>
#include <string.h>
#include <stdio.h>
#include <errno.h>
#include <unistd.h>
#include <assert.h>
#include <math.h>
#include <sys/types.h>
#include <sys/time.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <arpa/inet.h>

#define MAXBUF_LEN 1024

/*module private data definition*/
struct private_data_t {
	unsigned char* comm_buf;
	size_t comm_buf_size;
	struct jpeg_decompress_struct cinfo;
	struct jpeg_error_mgr jerr;
	unsigned char* jpeg_buf;
	size_t jpeg_buf_size;
};

/* implemented commands */
svs_command_t commands[CMD_MAX] = {
		{CMD_IMAGE_160x120, "a", 1},
		{CMD_IMAGE_320x240, "b", 1},
		{CMD_IMAGE_640x480, "c", 1},
		{CMD_IMAGE_1280x1024, "d", 1},
		{CMD_BATTERYSTAT, "D", 1},
		{CMD_GRABIMG, "I", 1},
		{CMD_MOTOR1, "M", 1},
		{CMD_MOTOR2, "m", 1},
		{CMD_STAT, "V", 1},
		{CMD_SERVO1, "S", 1},
		{CMD_SERVO2, "s", 1},
		{CMD_FAILSAFE, "F", 1},
		{CMD_RESET, "$!", 2},
};

/* implemented modes */
svs_image_mode_t image_modes[IMAGE_MODE_MAX] = {
		{ IMAGE_OFF, 0, 0, 0, 0, 0 },
		{ RGB888_160x120, CMD_IMAGE_160x120, 160, 120, 3, 24 }, //RGB888_160x120
		{ RGB888_320x240, CMD_IMAGE_320x240, 320, 240, 3, 24 }, //RGB888_320x240
		{ RGB888_640x480, CMD_IMAGE_640x480, 640, 480, 3, 24 }, //RGB888_640x480
		{ RGB888_1280x1024, CMD_IMAGE_1280x1024, 1280, 1024, 3, 24 }, //RGB888_1280x1024
		};

#ifndef DEBUG
#define DEBUG 0
#endif

#ifndef DEBUG_COMM
#define DEBUG_COMM 0
#endif

static int debug = DEBUG && DEBUG_COMM;

//HELPERS
int send_cmd(int socket_fd, svs_command_enum cmd_id, unsigned char* data, size_t data_size){
	int bytes_sent = 0;
	size_t cmd_buf_size = commands[cmd_id].cmd_size + data_size;
	unsigned char cmd_buf[cmd_buf_size];

	memcpy(cmd_buf,commands[cmd_id].cmd,commands[cmd_id].cmd_size);
	memcpy(cmd_buf+commands[cmd_id].cmd_size,data,data_size);

	if (debug) {
		fprintf(stdout, "%s: sent data\n",LINE_INFO);
		fhex_dump(stderr, cmd_buf, 0, cmd_buf_size);
	}
	if ((bytes_sent = send(socket_fd, cmd_buf, cmd_buf_size, 0)) < 0) {
		perror(LINE_INFO);
		return -1;
	}
	return bytes_sent;
}

/**
 * recv standard messages as specified in the protocol.
 * If message is longer than the provided buffer it will be truncated. In this case more data
 * could be waiting to be read.
 *
 * #X --> fixed size messages
 * ##....\r\n --> variable size messages
 * other messages  --> there are special messages that should be handled with the right routine
 *
 * \param socket_fd the socket to read from
 * \param buf buffer where the message will be stored
 * \param len max number of bytes read
 * \return number of bytes read
 */
int recv_message(int socket_fd, unsigned char* buf, size_t len){
	int read_bytes = 0;

	if ((read_bytes = recv(socket_fd, buf,2,MSG_WAITALL)) < 0 ){//at least we will read always 2 bytes
		perror(LINE_INFO);
		return -1;
	}

	if (buf[1] == '#'){ //read variable size message
		while (read_bytes < (int)len){
			int i;
			if ((i = recv(socket_fd, buf+read_bytes,len-read_bytes,0)) < 0 ){
				perror(LINE_INFO);
				return -1;
			}
			read_bytes += i;
			if (buf[read_bytes-2] == '\r' && buf[read_bytes-1] == '\n')//end of message
				break;
			//work around SRV-1 Blackfin - 07:16:37 - May 17 2009 put \n\r instead of \r\n
			if (buf[read_bytes-2] == '\n' && buf[read_bytes-1] == '\r')
				break;
		}
	}

	if (debug) {
		fprintf(stdout, "%s: recv data\n",LINE_INFO);
		fhex_dump(stderr, buf, 0, read_bytes);
	}

	return read_bytes;
}

//read all data available
int flush_socket(int socket_fd) {
	int read_bytes = 0, i;
	char buf[MAXBUF_LEN];

	while ((i = recv(socket_fd, buf, MAXBUF_LEN, MSG_DONTWAIT)) > 0) {//non blocking: FIXME
		read_bytes += i;
		if (debug){
			fprintf(stdout,"%s: discarded data %d bytes:\n",LINE_INFO, read_bytes);
			fhex_dump(stdout,buf,0,i);
		}
	}
	return read_bytes;
}

/**
 * recv image messages
 * \param socket_fd the socket to read from
 * \param buf buffer where the message will be stored
 * \param len size of buf to store message. If len < needed bytes for message return with error
 * \return number of bytes read or -1 if error
 */
int recv_img_message(int socket_fd, unsigned char* buf, size_t len) {
	int read_bytes = 0;
	unsigned char hdr_buf[10];//to read message header ##IMJxs0s1s2s3 = 10bytes

	int header_bytes = 10;

	if ((read_bytes = recv(socket_fd, hdr_buf, header_bytes, MSG_WAITALL)) < 0) {
		perror(LINE_INFO);
		goto recv_img_message_error;
	}

	if (debug){//print to stderr some bytes
		fprintf(stdout,"%s: recv image header: read %d bytes\n",LINE_INFO, read_bytes);
		fhex_dump(stdout,hdr_buf,0,read_bytes);
	}

	//parse header
	if (read_bytes < 10 || strncmp("##IMJ", (const char*) hdr_buf, 5) != 0) {
		fprintf(stderr, "%s: wrong image header\n", LINE_INFO);
		goto recv_img_message_error;
	}
	//char frame_npixels = hdr_buf[5];
	int frame_bytes = hdr_buf[6] + (hdr_buf[7] << 8) + (hdr_buf[8] << 16) + (hdr_buf[9] << 24);//little endian order

	if ((int)len < frame_bytes) {
		fprintf(stderr,
				"%s: buffer size (%d) not enough for image data (%d)\n",LINE_INFO, (int)len, frame_bytes);
		goto recv_img_message_error;
	}

	read_bytes = 0;
	while ((frame_bytes - read_bytes) > 0){
		int i;
		if ((i = recv(socket_fd, buf+read_bytes, frame_bytes-read_bytes, 0)) < 0){//err
			perror(LINE_INFO);
			goto recv_img_message_error;
		}
		read_bytes += i;
	}
	assert(read_bytes == frame_bytes);

	if (debug){//print to stderr some bytes
		fprintf(stdout,"%s: recv image data: read %d bytes\n",LINE_INFO, read_bytes);
		fhex_dump(stdout,buf,0,32);
		fprintf(stdout,"...\n");
		fhex_dump(stdout,buf,-32,read_bytes);
	}
	return read_bytes;
recv_img_message_error:
	flush_socket(socket_fd);
	return -1;
}


//IMPLEMENTATIONS
svs_comm_t* svs_create(const char *ip_addr, int master_port, int slave_port) {
	svs_comm_t* svs = (svs_comm_t*) malloc(sizeof(svs_comm_t));

	if (svs == 0) {
		fprintf(stderr, "%s: can't allocate memory\n", LINE_INFO);
		return 0;
	}

	svs->modules[SVS_MASTER].fd = -1;
	svs->modules[SVS_SLAVE].fd = -1;

	svs->modules[SVS_MASTER].image_mode = image_modes[IMAGE_OFF];
	svs->modules[SVS_MASTER].image_buf = 0;
	svs->modules[SVS_SLAVE].image_mode = image_modes[IMAGE_OFF];
	svs->modules[SVS_SLAVE].image_buf = 0;

	strncpy(svs->ip_addr, ip_addr, IP_ADDR_MAX_STR_LENGTH - 1);
	svs->modules[SVS_MASTER].port = master_port;
	svs->modules[SVS_SLAVE].port = slave_port;

	//abs max values
	svs->modules[SVS_MASTER].max_values_bank1[0] = 1.0;
	svs->modules[SVS_MASTER].max_values_bank1[1] = 1.0;
	svs->modules[SVS_MASTER].max_values_bank2[0] = 1.0;
	svs->modules[SVS_MASTER].max_values_bank2[1] = 1.0;
	svs->modules[SVS_SLAVE].max_values_bank1[0] = 1.0;
	svs->modules[SVS_SLAVE].max_values_bank1[1] = 1.0;
	svs->modules[SVS_SLAVE].max_values_bank2[0] = 1.0;
	svs->modules[SVS_SLAVE].max_values_bank2[1] = 1.0;

	//master private data
	svs->modules[SVS_MASTER].priv = (private_data_t*) malloc(sizeof(private_data_t));
	svs->modules[SVS_MASTER].priv->comm_buf = (unsigned char*)malloc(sizeof(unsigned char)*MAXBUF_LEN);
	svs->modules[SVS_MASTER].priv->comm_buf_size = MAXBUF_LEN;
	svs->modules[SVS_MASTER].priv->jpeg_buf = 0;
	//init jpeg decompression engine
	svs->modules[SVS_MASTER].priv->cinfo.err = jpeg_std_error(&svs->modules[SVS_MASTER].priv->jerr);
	jpeg_create_decompress(&(svs->modules[SVS_MASTER].priv->cinfo));

	//slave private data
	svs->modules[SVS_SLAVE].priv = (private_data_t*) malloc(sizeof(private_data_t));
	svs->modules[SVS_SLAVE].priv->comm_buf = (unsigned char*)malloc(sizeof(unsigned char)*MAXBUF_LEN);
	svs->modules[SVS_SLAVE].priv->comm_buf_size = MAXBUF_LEN;
	svs->modules[SVS_SLAVE].priv->jpeg_buf = 0;
	//init jpeg decompression engine
	svs->modules[SVS_SLAVE].priv->cinfo.err = jpeg_std_error(&svs->modules[SVS_SLAVE].priv->jerr);
	jpeg_create_decompress(&(svs->modules[SVS_SLAVE].priv->cinfo));

	return svs;
}

void svs_destroy(svs_comm_t* r) {
	if (r) {
		if (r->modules[SVS_MASTER].image_buf != 0) {
			free(r->modules[SVS_MASTER].image_buf);
			free(r->modules[SVS_MASTER].priv->jpeg_buf);
		}
		if (r->modules[SVS_SLAVE].image_buf != 0 ) {
			free(r->modules[SVS_SLAVE].image_buf);
			free(r->modules[SVS_SLAVE].priv->jpeg_buf);
		}
		free(r->modules[SVS_MASTER].priv->comm_buf);
		free(r->modules[SVS_SLAVE].priv->comm_buf);
		jpeg_destroy_decompress(&(r->modules[SVS_MASTER].priv->cinfo));
		jpeg_destroy_decompress(&(r->modules[SVS_SLAVE].priv->cinfo));
		free(r->modules[SVS_MASTER].priv);
		free(r->modules[SVS_SLAVE].priv);
		free(r);
	}
}

int svs_open(svs_comm_t* r) {
	assert(r);

	struct sockaddr_in addr;
	int optval;

	if (r->modules[SVS_MASTER].fd >= 0) {
		fprintf(stderr, "%s: connection already open\n", LINE_INFO);
		goto svs_open_error;
	}

	fprintf(stdout,
			"Opening connection to SVS on %s [master:%d,slave:%d]...\n",
			r->ip_addr, r->modules[SVS_MASTER].port, r->modules[SVS_SLAVE].port);

	//create sockets
	if ((r->modules[SVS_MASTER].fd = socket(AF_INET, SOCK_STREAM, 0)) < 0) {
		perror(LINE_INFO);
		goto svs_open_error;
	}
	if ((r->modules[SVS_SLAVE].fd = socket(AF_INET, SOCK_STREAM, 0)) < 0) {
		perror(LINE_INFO);
		goto svs_open_error;
	}

	//connect to svs system
	addr.sin_family = AF_INET;

	//master
	addr.sin_port = htons(r->modules[SVS_MASTER].port);
	inet_pton(AF_INET, r->ip_addr, &addr.sin_addr);
	if (connect(r->modules[SVS_MASTER].fd, (const struct sockaddr*) &addr, sizeof(addr)) < 0) {
		perror(LINE_INFO);
		goto svs_open_error;
	}

	//slave
	addr.sin_port = htons(r->modules[SVS_SLAVE].port);
	inet_pton(AF_INET, r->ip_addr, &addr.sin_addr);
	if (connect(r->modules[SVS_SLAVE].fd, (const struct sockaddr*) &addr, sizeof(addr)) < 0) {
		perror(LINE_INFO);
		goto svs_open_error;
	}

	//Set keepalive
	optval = SVS_COMM_KEEPALIVE;
	if (setsockopt(r->modules[SVS_MASTER].fd, SOL_SOCKET, SO_KEEPALIVE, (const void*)&optval, sizeof(optval)) < 0) {
		perror(LINE_INFO);
		goto svs_open_error;
	}
	if (setsockopt(r->modules[SVS_SLAVE].fd, SOL_SOCKET, SO_KEEPALIVE, (const void*)&optval, sizeof(optval)) < 0) {
		perror(LINE_INFO);
		goto svs_open_error;
	}

	//Set rcv/send timeouts
	struct timeval tv;
	tv.tv_sec = (int)SVS_COMM_TIMEOUT_MS/1000;
	tv.tv_usec = (SVS_COMM_TIMEOUT_MS - (tv.tv_sec*1000)) * 1000;
	if (setsockopt(r->modules[SVS_MASTER].fd, SOL_SOCKET, SO_RCVTIMEO, (const void*) &tv, sizeof(tv)) < 0) {
		perror(LINE_INFO);
		goto svs_open_error;
	}
	if (setsockopt(r->modules[SVS_SLAVE].fd, SOL_SOCKET, SO_RCVTIMEO,(const void*) &tv, sizeof(tv)) < 0) {
		perror(LINE_INFO);
		goto svs_open_error;
	}


	return 0;

svs_open_error:
	svs_close(r);
	return -1;
}

int svs_reset(svs_comm_t* r){
	assert(r);
	int read_bytes = 0;

	if (debug)
		fprintf(stdout, "svs reseting...\n");

	//flush sockets to discard any data waiting to be received
	flush_socket(r->modules[SVS_MASTER].fd);
	flush_socket(r->modules[SVS_SLAVE].fd);

	//send $! command to both servers
	if (send_cmd(r->modules[SVS_MASTER].fd, CMD_RESET, 0, 0) < 0) {
		fprintf(stderr, "%s: error sending reset cmd to master\n",LINE_INFO);
		return -1;
	}
	//send $! command to both servers
	if (send_cmd(r->modules[SVS_SLAVE].fd, CMD_RESET, 0, 0) < 0) {
		fprintf(stderr, "%s: error sending reset cmd to slave\n",LINE_INFO);
		return -1;
	}

	//wait a sec for the reset
	usleep(1000000);

	//read #f
	if ((read_bytes = recv_message(r->modules[SVS_MASTER].fd, r->modules[SVS_MASTER].priv->comm_buf, r->modules[SVS_MASTER].priv->comm_buf_size)) < 0) {
		fprintf(stderr, "%s: error receiving reset cmd reply from master\n",LINE_INFO);
		return -1;
	}
	//read ##Version
	if ((read_bytes = recv_message(r->modules[SVS_MASTER].fd, r->modules[SVS_MASTER].priv->comm_buf, r->modules[SVS_MASTER].priv->comm_buf_size)) < 0) {
		fprintf(stderr, "%s: error receiving reset cmd reply from master\n",LINE_INFO);
		return -1;
	}
	//read #f
	if ((read_bytes = recv_message(r->modules[SVS_SLAVE].fd, r->modules[SVS_SLAVE].priv->comm_buf, r->modules[SVS_SLAVE].priv->comm_buf_size)) < 0) {
		fprintf(stderr, "%s: error receiving reset cmd reply from slave\n",LINE_INFO);
		return -1;
	}
	//read ##Version
	if ((read_bytes = recv_message(r->modules[SVS_SLAVE].fd, r->modules[SVS_SLAVE].priv->comm_buf, r->modules[SVS_SLAVE].priv->comm_buf_size)) < 0) {
		fprintf(stderr, "%s: error receiving reset cmd reply from slave\n",LINE_INFO);
		return -1;
	}
	return 0;
}

int svs_init(svs_comm_t* r) {
	assert(r);

	//open connection
	if (svs_open(r) < 0) {
		fprintf(stderr, "%s: can't open SVS connection\n", LINE_INFO);
		goto svs_init_error;
	}

	//send a reset
	if (svs_reset(r) != 0)
		goto svs_init_error;

	return 0;
svs_init_error:
	svs_close(r);
	return -1;
}

//int svs_reset(svs_comm_t* r, double failsafe_timeout);
int svs_close(svs_comm_t* r) {
	assert(r);

	if (r->modules[SVS_MASTER].fd >= 0)
		close(r->modules[SVS_MASTER].fd);
	if (r->modules[SVS_SLAVE].fd >= 0)
		close(r->modules[SVS_SLAVE].fd);
	r->modules[SVS_MASTER].fd = -1;
	r->modules[SVS_SLAVE].fd = -1;
	return 0;
}

svs_battery_stat_enum svs_get_battery_status(svs_comm_t* r) {
	assert(r);
	int read_bytes = 0;

	//send D to master
	if (send_cmd(r->modules[SVS_MASTER].fd, CMD_BATTERYSTAT, 0, 0) < 0) {
		fprintf(stderr, "%s: error sending battery stat cmd to master\n", LINE_INFO);
		return -1;
	}
	if ((read_bytes = recv_message(r->modules[SVS_MASTER].fd, r->modules[SVS_MASTER].priv->comm_buf, r->modules[SVS_MASTER].priv->comm_buf_size)) < 0) {
		fprintf(stderr, "%s: error receiving battery stat cmd from master\n", LINE_INFO);
		return -1;
	}

	//end string to perform search
	int endpos;
	if (read_bytes < (int)r->modules[SVS_MASTER].priv->comm_buf_size)
		endpos = read_bytes;
	else
		endpos = r->modules[SVS_MASTER].priv->comm_buf_size - 1;
	r->modules[SVS_MASTER].priv->comm_buf[endpos] = '\0';
	//parse reply ##D - ...okay or ##D - low....
	if (strstr((const char*)r->modules[SVS_MASTER].priv->comm_buf, "low")) {
		return BATTERY_LOW;
	}
	return BATTERY_OK;
}

int svs_set_image_mode(svs_module_t* module, svs_image_mode_enum mode) {
	assert(module && mode != IMAGE_OFF);
	int read_bytes;

	if (module->image_mode.mode_id != mode) {
		//send message to set new mode
		if (send_cmd(module->fd, image_modes[mode].cmd_id, 0, 0) != 1) {
			fprintf(stderr, "%s: error sending image mode cmd to svs:%d\n", LINE_INFO,module->port);
			return -1;
		}
		if ((read_bytes=recv_message(module->fd, module->priv->comm_buf, module->priv->comm_buf_size)) < 0) {
			fprintf(stderr, "%s: error receiving image mode cmd from svs:%d\n", LINE_INFO,module->port);
			return -1;
		}

		//update internal state
		module->image_mode = image_modes[mode];
		//realloc image buffers to new sizes
		module->image_buf_size = module->image_mode.width * module->image_mode.height * module->image_mode.channels;
		module->image_buf = (unsigned char*) realloc(module->image_buf, module->image_buf_size);
		//jpeg data is expected to be smaller that image_buf_size
		module->priv->jpeg_buf_size = module->image_buf_size;
		module->priv->jpeg_buf = (unsigned char*) realloc(module->priv->jpeg_buf,module->image_buf_size);
	}
	return 0;
}


const unsigned char* svs_get_image(svs_module_t* module) {
	assert(module);
	int read_bytes, row_stride, row_count;
	JSAMPARRAY row_buffer;

	//image mode initialized?
	if (module->image_mode.mode_id == IMAGE_OFF){
		fprintf(stderr, "%s: image mode not initialized\n",LINE_INFO);
		return 0;
	}

	//send I command and read images
	if (send_cmd(module->fd, CMD_GRABIMG, 0, 0) < 0) {
		fprintf(stderr, "%s: error sending grab image cmd to master\n",LINE_INFO);
		return 0;
	}
	if ((read_bytes = recv_img_message(module->fd, module->priv->jpeg_buf, module->priv->jpeg_buf_size)) < 0){
		fprintf(stderr, "%s: error receiving grab image cmd from master\n",LINE_INFO);
		return 0;
	}

	//if (read_bytes == 0) {//FIXME: retry?
		//from protocol description: Note that sometimes the 'I' command returns nothing if the robot
		//camera is busy, so the 'I' command should be called as many times as needed until a frame is returned

	//}

	//convet jpeg to rgb
	struct jpeg_decompress_struct* cinfo =  &module->priv->cinfo;

	jpeg_buffer_src(cinfo, module->priv->jpeg_buf, read_bytes);
	jpeg_read_header(cinfo, TRUE);
	jpeg_start_decompress(cinfo);
	assert(cinfo->output_width == module->image_mode.width);
	assert(cinfo->output_height == module->image_mode.height);
	assert(cinfo->output_components == (int)module->image_mode.channels);

	//allocate a buffer to read rows
	row_stride = cinfo->output_width * cinfo->output_components;
	row_count = 0;
	/* Make a one-row-high sample array that will go away when done with image */
	row_buffer = (*cinfo->mem->alloc_sarray)((j_common_ptr) cinfo, JPOOL_IMAGE, row_stride, 1);
	while (cinfo->output_scanline < cinfo->output_height) {
		jpeg_read_scanlines(cinfo, row_buffer, 1);
		memcpy(module->image_buf+(row_count*row_stride),row_buffer[0],row_stride);
		row_count++;
	}
	jpeg_finish_decompress(cinfo);

	return module->image_buf;
}

//clamp x to the range [a,b]
float clampf(float x, float a, float b){
	return fmaxf(a,fminf(b,x));
}

int svs_set_motor_speeds(svs_module_t* module, int bank, float left, float right, int durationms ){
	assert(module);
	assert(bank == 1 || bank == 2);

	svs_command_enum cmd;
	unsigned char data_buf[3];
	char _left,_right;
	unsigned char _duration;
	int read_bytes = 0;

	if (debug) {
		fprintf(stdout, "svs:%d(bank=%d) motor set to speed: left=%lf right=%lf\n",module->port,bank,left,right);
	}

	//M command use 1 signed byte for speed values. So we have to go from [-1.0,1.0] -> [-127,127]
	_left = (char)(left*127.0);
	_right = (char)(right*127.0);

	//M command use 1 unsigned byte for duration in 10*ms units. So we have to go from [0,inf) / 10 -> [0,255]
	_duration = (unsigned char)(fminf(durationms/10.0,255.0));

	if (bank == 1)
		cmd = CMD_MOTOR1;
	else
		cmd = CMD_MOTOR2;
	data_buf[0] = (unsigned char) _left;
	data_buf[1] = (unsigned char) _right;
	data_buf[2] = _duration;

	if (send_cmd(module->fd, cmd, data_buf, sizeof(data_buf)) < 0) {
		fprintf(stderr, "%s: error sending motor cmd to svs:%d(bank=%d)\n", LINE_INFO,module->port,bank);
		return -1;
	}
	if ((read_bytes = recv_message(module->fd, module->priv->comm_buf, module->priv->comm_buf_size)) < 0) {
		fprintf(stderr, "%s: error receiving motor cmd reply from svs:%d\n", LINE_INFO, module->port);
		return -1;
	}

	return 0;
}

int svs_set_motor1_speeds(svs_module_t* module, float left, float right, int durationms ){
	//clamp values
	left = clampf(left, -(module->max_values_bank1[0]), module->max_values_bank1[0]);
	right = clampf(right, -(module->max_values_bank1[1]), module->max_values_bank1[1]);
	return svs_set_motor_speeds(module,1,left,right,durationms);
}

int svs_set_motor1_max_speeds(svs_module_t* module, float left, float right){
	assert(module);
	module->max_values_bank1[0] = clampf(left,-1.0,1.0);
	module->max_values_bank1[1] = clampf(right,-1.0,1.0);
	return 0;
}


int svs_set_motor2_speeds(svs_module_t* module, float left, float right, int durationms ){
	left = clampf(left, -(module->max_values_bank2[0]), module->max_values_bank2[0]);
	right = clampf(right, -(module->max_values_bank2[1]), module->max_values_bank2[1]);
	return svs_set_motor_speeds(module,2,left,right,durationms);
}

int svs_set_motor2_max_speeds(svs_module_t* module, float left, float right){
	assert(module);
	module->max_values_bank2[0] = clampf(left,-1.0,1.0);
	module->max_values_bank2[1] = clampf(right,-1.0,1.0);
	return 0;
}

int svs_set_servo_values(svs_module_t* module, int bank, float left, float right){
	assert(module);
	assert(bank == 1 || bank == 2);

	svs_command_enum cmd;
	unsigned char data_buf[2];
	char _left,_right;
	int read_bytes = 0;

	if (debug) {
		fprintf(stdout, "svs:%d(bank=%d) servo set to value: left=%lf right=%lf\n",module->port,bank,left,right);
	}

	//S command use values from 1 to 100, being 50 the middle point. So we have to go from [-1.0,1.0] -> [1,100]
	double aux_l, aux_r;

	aux_l = (((left + 1.0) / 2.0) * 99.0) + 1.0;
	_left = (char) aux_l;
	aux_r = (((right + 1.0) / 2.0) * 99.0) + 1.0;
	_right = (char) aux_r;

	if (bank == 1)
		cmd = CMD_SERVO1;
	else
		cmd = CMD_SERVO2;
	data_buf[0] = (unsigned char) _left;
	data_buf[1] = (unsigned char) _right;

	if (send_cmd(module->fd, cmd, data_buf, sizeof(data_buf)) < 0) {
		fprintf(stderr, "%s: error sending servo cmd to svs:%d(bank=%d)\n", LINE_INFO,module->port,bank);
		return -1;
	}
	if ((read_bytes = recv_message(module->fd, module->priv->comm_buf, module->priv->comm_buf_size)) < 0) {
		fprintf(stderr, "%s: error receiving servo cmd reply from svs:%d\n", LINE_INFO, module->port);
		return -1;
	}

	return 0;
}

int svs_set_servo1_values(svs_module_t* module, float left, float right ){
	//clamp values
	left = clampf(left, -(module->max_values_bank1[0]), module->max_values_bank1[0]);
	right = clampf(right, -(module->max_values_bank1[1]), module->max_values_bank1[1]);
	return svs_set_servo_values(module,1,left,right);
}

int svs_set_servo1_max_values(svs_module_t* module, float left, float right){
	assert(module);
	module->max_values_bank1[0] = clampf(left,-1.0,1.0);
	module->max_values_bank1[1] = clampf(right,-1.0,1.0);
	return 0;
}

int svs_set_servo2_values(svs_module_t* module, float left, float right ){
	left = clampf(left, -(module->max_values_bank2[0]), module->max_values_bank2[0]);
	right = clampf(right, -(module->max_values_bank2[1]), module->max_values_bank2[1]);
	return svs_set_servo_values(module,2,left,right);
}

int svs_set_servo2_max_values(svs_module_t* module, float left, float right){
	assert(module);
	module->max_values_bank2[0] = clampf(left,-1.0,1.0);
	module->max_values_bank2[1] = clampf(right,-1.0,1.0);
	return 0;
}


//Renegade stuff

/* int renegadesvs_init_motors(svs_comm_t* r) { */
/* 	assert(r); */
/* 	unsigned char data_buf[2]; */
/* 	int read_bytes = 0; */

/* 	//renegade base use S command to control motors. */
/* 	//The range for this command is 1-100 (see firmware srv.c at setPPM1()) */
/* 	//being 50 the 0 point */
/* 	data_buf[0] = 50; */
/* 	data_buf[1] = 50; */
/* 	if (send_cmd(r->modules[SVS_MASTER].fd, CMD_FAILSAFE, data_buf, sizeof(data_buf)) < 0) { */
/* 		fprintf(stderr, "%s: error sending failsafe cmd to master\n",LINE_INFO); */
/* 		return -1; */
/* 	} */
/* 	if ((read_bytes = recv_message(r->modules[SVS_MASTER].fd, r->priv->comm_buffer, r->priv->comm_buffer_len)) < 0){ */
/* 		fprintf(stderr, "%s: error receiving failsafe cmd reply from master\n",LINE_INFO); */
/* 		return -1; */
/* 	} */
/* 	return 0; */
/* } */


/* int renegadesvs_set_speeds(svs_comm_t* r, double tv, double rv) { */
/* 	assert(r); */
/* 	unsigned char data_buf[2]; */
/* 	char _tv,_rv; */
/* 	int read_bytes = 0; */

/* 	if (debug) { */
/* 		fprintf(stdout, "svs master motor set to speed: t=%lf r=%lf\n",tv,rv); */
/* 	} */

/* 	if (tv < -1.0) tv = -1.0; */
/* 	else if (tv > 1.0) tv = 1.0; */

/* 	if (rv < -1.0) rv = -1.0; */
/* 	else if (rv > 1.0) rv = 1.0; */


/* 	//renegade base use S command to control motors. */
/* 	//The range for this command is 1-100 (see firmware srv.c at setPPM1()) */
/* 	//being 50 the 0 point. So we have to change range from [-1 1] to [1 100] */
/* 	double aux_t,aux_r; */

/* 	// */

/* 	aux_t = (((tv+1.0)/2.0) * 99.0) + 1.0; */
/* 	_tv = (char)aux_t; */
/* 	aux_r = (((rv+1.0)/2.0) * 99.0) + 1.0; */
/* 	_rv = (char)aux_r; */

/* 	data_buf[0] = (unsigned char)_tv; */
/* 	data_buf[1] = (unsigned char)_rv; */
/* 	if (send_cmd(r->modules[SVS_MASTER].fd, CMD_SERVO1, data_buf, sizeof(data_buf)) < 0) { */
/* 		fprintf(stderr, "%s: error sending motor cmd to master\n", LINE_INFO); */
/* 		return -1; */
/* 	} */
/* 	if ((read_bytes = recv_message(r->modules[SVS_MASTER].fd, r->priv->comm_buffer,r->priv->comm_buffer_len)) < 0) { */
/* 		fprintf(stderr, "%s: error receiving motor cmd reply from master\n",LINE_INFO); */
/* 		return -1; */
/* 	} */
/* 	return 0; */
/* } */

/* int renegadesvs_set_pantilt(svs_comm_t* r, double pan, double tilt) { */
/* 	assert(r); */

/* 	return 0; */
/* } */

/* int renegadesvs_get_position(svs_comm_t* r) {//TODO: implement */
/* 	assert(r); */

/* 	return 0; */
/* } */
